#include "rclcpp/rclcpp.hpp"       
#include "geometry_msgs/msg/twist.hpp" 
#include "geometry_msgs/msg/twist_stamped.hpp"
#include <thread>
#include <map>
#include <termios.h>
#include <unistd.h>
#include <sys/select.h>

using namespace std::chrono_literals;

// 跨平台键盘输入处理
int getch() {
    struct termios oldt, newt;
    int ch;
    int fd = STDIN_FILENO;
    tcgetattr(fd, &oldt);
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(fd, TCSANOW, &newt);
    ch = getchar();
    tcsetattr(fd, TCSANOW, &oldt);
    return ch;
}

class RobotControl : public rclcpp::Node {
public:
    RobotControl() : Node("myrobot_control") {
        // 声明参数 
        stamped_ = this->declare_parameter<bool>("stamped", false);
        frame_id_ = this->declare_parameter<std::string>("frame_id", "");
        
        // 参数有效性检查 
        if (!stamped_ && !frame_id_.empty()) {
            throw std::runtime_error("'frame_id' can only be set when 'stamped' is true");
        }

        // 创建发布者 
        if (stamped_) {
            stamped_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>("myrobot_control", 10);
            stamped_msg_.header.frame_id = frame_id_;
        } else {
            twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("myrobot_control", 10);
        }

        // 初始化速度参数 
        speed_ = 0.5;
        turn_ = 1.0;
        x_=0, y_=0, z_=0, th_=0;
    }

    void run() {
        // 打印控制说明
const char* msg = R"(

Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

For Holonomic mode (strafing), hold down the shift key:
---------------------------
   U    I    O
   J    K    L
   M    <    >

t : up (+z)
b : down (-z)

anything else : stop

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%

CTRL-C to quit

)";
        std::cout << msg << std::endl;

        // 主循环
        while (rclcpp::ok()) {
            int key = getch();
            processKey(key);
            publishVelocity();
            rclcpp::spin_some(shared_from_this());
        }
    }

private:
    void processKey(int key) {
        // 运动方向绑定 
        static const std::map<int, std::vector<double>> move_bindings = {
            {'i', {1, 0, 0, 0}},   // Left
            {'o', {1, 0, 0, -1}},  // Right
            {'j', {0, 0, 0, 1}},   // Forward
            {'l', {0, 0, 0, -1}},  // Backward
            {'u', {1, 0, 0, 1}},
            {',', {-1, 0, 0, 0}},
            {'.', {-1, 0, 0, 1}},
            {'m', {-1, 0, 0, -1}},
            {'O', {1, -1, 0, 0}},
            {'I', {1, 0, 0, 0}},
            {'J', {0, 1, 0, 0}},
            {'L', {0, -1, 0, 0}},
            {'U', {1, 1, 0, 0}},
            {'<', {-1, 0, 0, 0}},
            {'>', {-1, -1, 0, 0}},
            {'M', {-1, 1, 0, 0}},
            {'t', {0, 0, 1, 0}},
            {'b', {0, 0, -1, 0}},

        };

        // 速度调整绑定
        static const std::map<int, std::vector<double>> speed_bindings = {
            {'q', {1.1, 1.1}},
            {'z', {0.9, 0.9}},
            {'w', {1.1, 1}},
            {'x', {0.9, 1}},
            {'e', {1, 1.1}},
            {'c', {1, 0.9}},
            
        };

        if (move_bindings.count(key)) {
            const auto& cmd = move_bindings.at(key);
            x_ = cmd[0];
            y_ = cmd[1];
            z_ = cmd[2];
            th_ = cmd[3];
        } 
        else if (speed_bindings.count(key)) {
            const auto& factor = speed_bindings.at(key);
            speed_ *= factor[0];
            turn_ *= factor[1];
            std::cout << "\rCurrent: speed " << speed_ << "\tturn " << turn_ << std::flush;
        } 
        else if (key == 3) { // Ctrl+C 
            stopRobot();
            rclcpp::shutdown();
        } else {
            x_ = y_ = z_ = th_ = 0;
        }
    }

    void publishVelocity() {
        // 更新时间戳 
        if (stamped_) {
            stamped_msg_.header.stamp = now();
            geometry_msgs::msg::Twist& twist = stamped_msg_.twist;
            updateTwistFields(twist);
            stamped_pub_->publish(stamped_msg_);
        } else {
            geometry_msgs::msg::Twist twist;
            updateTwistFields(twist);
            twist_pub_->publish(twist);
        }
    }

    void updateTwistFields(geometry_msgs::msg::Twist& twist) {
        twist.linear.x = x_ * speed_;
        twist.linear.y = y_ * speed_;
        twist.linear.z = z_ * speed_;
        twist.angular.x = 0.0;
        twist.angular.y = 0.0;
        twist.angular.z = th_ * turn_;
    }

    void stopRobot() {
        // 发布零速度 
        if (stamped_) {
            stamped_msg_.twist = geometry_msgs::msg::Twist();
            stamped_pub_->publish(stamped_msg_);
        } else {
            auto twist = geometry_msgs::msg::Twist();
            twist_pub_->publish(twist);
        }
    }

    // 成员变量
    bool stamped_;
    std::string frame_id_;
    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr stamped_pub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
    geometry_msgs::msg::TwistStamped stamped_msg_;
    double speed_, turn_;
    double x_=0, y_=0, z_=0, th_=0;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<RobotControl>();
    node->run();
    return 0;
}